/*
	init.S
	
	Startup code for MIPS32
	
	Meng Sun(c) leon.meng.sun@gmail.com
	Source code released under LGPLv3
	https://www.gnu.org/licenses/lgpl-3.0.txt
*/

.global INITSSAVEENVSLOT
.local INITSSAVEENVSLOTINBYTE
.set INITSSAVEENVSLOT, 34 /*number of 32 bits registers need to be saved*/
.set INITSSAVEENVSLOTINBYTE, (INITSSAVEENVSLOT + 4) * 4

/*define macro used for save cpu environment, gpr*/
.macro INITSSAVEGPR reg, slot
	sw $\reg, (INITSSAVEENVSLOTINBYTE - \slot * 4)($sp)
.endm

/*define macro used for save cpu environment, cp0*/
.macro INITSSAVECP0 reg, sel, slot
	mfc0 $t0, $\reg, \sel
	sw $t0, (INITSSAVEENVSLOTINBYTE - \slot * 4)($sp)
.endm

/*define macro used for restore cpu environment, gpr*/
.macro INITSRESTOREGPR reg, slot
	lw $\reg, (INITSSAVEENVSLOTINBYTE - \slot * 4)($sp)
.endm

/*define macro used for restore cpu environment, cp0*/
.macro INITSRESTORECP0 reg, sel, slot
	lw $t0, (INITSSAVEENVSLOTINBYTE - \slot * 4)($sp)
	mtc0 $t0, $\reg, \sel
.endm

.section .tRESET, "ax"
RESET:	
	li $sp, 0xa1000000
	li $fp, 0xa1000000
	li $ra, 0xbfc00000
	jal bootld
	la $t0, mips32r2ResetEntry
	jalr $t0
_RESETWAIT:
	wait
	ssnop
	j _RESETWAIT

.section .tTLBREFEXL0, "ax"
TLBREFEXL0:
	j 0xbfc00400

.section .tTLBREFEXL1_AOTH, "ax"
TLBREFEXL1:
	j 0xbfc00400

.section .tCACHEERR, "ax"
CACHEERR:
	eret

.section .tINTERRUPT, "ax"
INTERRUPT:
	di
	
	addiu $sp, $sp, -1 * INITSSAVEENVSLOTINBYTE	

	/*save environment*/
	INITSSAVEGPR a0, 1
	INITSSAVEGPR a1, 2
	INITSSAVEGPR a2, 3
	INITSSAVEGPR a3, 4 /*4 x function arguments*/
	INITSSAVEGPR v0, 5
	INITSSAVEGPR v1, 6 /*2 x function return value*/
	INITSSAVEGPR fp, 7 /*frame ptr*/
	INITSSAVEGPR at, 8 /*assembler temp*/
	INITSSAVEGPR sp, 9 /*stack ptr*/
	INITSSAVEGPR gp, 10 /*global ptr*/
	INITSSAVEGPR ra, 11 /*return addr*/
	INITSSAVEGPR k0, 12
	INITSSAVEGPR k1, 13 /*2 x kernel reserved*/
	INITSSAVEGPR s0, 14
	INITSSAVEGPR s1, 15
	INITSSAVEGPR s2, 16
	INITSSAVEGPR s3, 17
	INITSSAVEGPR s4, 18
	INITSSAVEGPR s5, 19
	INITSSAVEGPR s6, 20
	INITSSAVEGPR s7, 21 /*7 x saved temp*/
	INITSSAVEGPR t0, 22
	INITSSAVEGPR t1, 23
	INITSSAVEGPR t2, 24
	INITSSAVEGPR t3, 25
	INITSSAVEGPR t4, 26
	INITSSAVEGPR t5, 27
	INITSSAVEGPR t6, 28
	INITSSAVEGPR t7, 29
	INITSSAVEGPR t8, 30
	INITSSAVEGPR t9, 31 /*10 x temp*/
	
	INITSSAVECP0 12, 0, 32 /*cp0 status*/
	INITSSAVECP0 14, 0, 33 /*cp0 epc*/
	INITSSAVECP0 30, 0, 34 /*cp0 errorpc*/
	
	move $fp, $sp

	mfc0 $a0, $13, 0 /*load cp0 cause value as first param*/
	addi $a1, $fp, 0 /*load fp register value as second param*/
	li $t0, 0x0000ff00
	and $t0, $a0, $t0 /*get activated ip bits in cause register in $t0*/
	mfc0 $t1, $12, 0
	xor $t1, $t1, $t0 /*disable acitvated interrupt from cp0 status*/
	li $t0, 0xffffffe1
	and $t1, $t1, $t0 /*clear KSU, ERL and EXL*/
	mtc0 $t1, $12, 0 /*write back to cp0 status*/

	ei

	la $t0, mips32r2InterruptService
	jalr $t0

	di

	move $sp, $fp

	/*restore environment*/
	INITSRESTORECP0 12, 0, 32 /*cp0 status*/
	
	lw $t0, (INITSSAVEENVSLOTINBYTE - 33 * 4)($sp) /*load cp0 epc*/
	addu $t0, $t0, $v0 /*epc += 4 for sync. exception; epc += 0 for async. exception*/
	mtc0 $t0, $14, 0
	
	INITSRESTORECP0 30, 0, 34 /*cp0 errorpc*/

	INITSRESTOREGPR a0, 1
	INITSRESTOREGPR a1, 2
	INITSRESTOREGPR a2, 3
	INITSRESTOREGPR a3, 4 /*4 x function arguments*/
	INITSRESTOREGPR v0, 5
	INITSRESTOREGPR v1, 6 /*2 x function return value*/
	INITSRESTOREGPR fp, 7 /*frame ptr*/
	INITSRESTOREGPR at, 8 /*assembler temp*/
	INITSRESTOREGPR sp, 9 /*stack ptr*/
	INITSRESTOREGPR gp, 10 /*global ptr*/
	INITSRESTOREGPR ra, 11 /*return addr*/
	INITSRESTOREGPR k0, 12
	INITSRESTOREGPR k1, 13 /*2 x kernel reserved*/
	INITSRESTOREGPR s0, 14
	INITSRESTOREGPR s1, 15
	INITSRESTOREGPR s2, 16
	INITSRESTOREGPR s3, 17
	INITSRESTOREGPR s4, 18
	INITSRESTOREGPR s5, 19
	INITSRESTOREGPR s6, 20
	INITSRESTOREGPR s7, 21 /*7 x saved temp*/
	INITSRESTOREGPR t0, 22
	INITSRESTOREGPR t1, 23
	INITSRESTOREGPR t2, 24
	INITSRESTOREGPR t3, 25
	INITSRESTOREGPR t4, 26
	INITSRESTOREGPR t5, 27
	INITSRESTOREGPR t6, 28
	INITSRESTOREGPR t7, 29
	INITSRESTOREGPR t8, 30
	INITSRESTOREGPR t9, 31 /*10 x temp*/

	addiu $sp, $sp, INITSSAVEENVSLOTINBYTE

	ei

	eret

